// Include files.
#include "TestDrive.hpp"

TestDrive::TestDrive() :
	MecanumDriveSubsystem
	(
		new CANJaguar(CAN_POSITION_DRIVE_LEFT_FRONT),
		new CANJaguar(CAN_POSITION_DRIVE_LEFT_BACK),
		new CANJaguar(CAN_POSITION_DRIVE_RIGHT_FRONT),
		new CANJaguar(CAN_POSITION_DRIVE_RIGHT_BACK)
	)
{
}

void TestDrive::Initialize()
{
	printf("Test Drive Subsystem: Initalizing\n");
	
	SetMotorInvertation(MotorPositionLeftFront, false);
	SetMotorInvertation(MotorPositionLeftBack, false);
	SetMotorInvertation(MotorPositionRightFront, true);
	SetMotorInvertation(MotorPositionRightBack, true);

	SetSpeedReference(SpeedReferenceQuadEncoder);
	SetDistanceReference(DistanceReferenceQuadEncoder);
	
	SetEncoderCodesPerRevolution(MotorPositionLeftFront, 360);
	SetEncoderCodesPerRevolution(MotorPositionLeftBack, 250);
	SetEncoderCodesPerRevolution(MotorPositionRightFront, 250);
	SetEncoderCodesPerRevolution(MotorPositionRightBack, 250);
	
	SetMaximumOutput(450);
	
	SetControlMode(DriveSubsystem::ControlModeSpeed);
	
	SetDistanceThreshold(0.5);
	
	SetPID(ControlModeSpeed, MotorPositionLeftFront, 0.4, 0.001, 0.0);
	SetPID(ControlModeSpeed, MotorPositionLeftBack, 0.4, 0.001, 0.0);
	SetPID(ControlModeSpeed, MotorPositionRightFront, 0.4, 0.001, 0.0);
	SetPID(ControlModeSpeed, MotorPositionRightBack, 0.4, 0.001, 0.0);
	
	SetPID(ControlModeDistance, MotorPositionLeftFront, 60.0, 0.001, 0.0);
	SetPID(ControlModeDistance, MotorPositionLeftBack, 60.0, 0.001, 0.0);
	SetPID(ControlModeDistance, MotorPositionRightFront, 60.0, 0.001, 0.0);
	SetPID(ControlModeDistance, MotorPositionRightBack, 60.0, 0.001, 0.0);
	
	SetExpiration(0.5);
	
	printf("Test Drive Subsystem: Initialized\n");
}
